/******************** (C) COPYRIGHT 2007 STMicroelectronics ********************
* File Name          : stm32f10x_encoder.c
* Author             : IMS Systems Lab
* Date First Issued  : 21/11/07
* Description        : This file contains the software implementation for the
*                      encoder unit
********************************************************************************
* History:
* 21/11/07 v1.0
********************************************************************************
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "Y_encoder.h"
#include "stm32f10x_tim.h"
#include "misc.h"
#include "drv.h"
#include "shell.h"
#include "y_pwm.h"
#include "fpga.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/


volatile int   Y_Encoder_Timer_Overflow=0;
volatile unsigned short   Y_Encoder_Old_Counter=0;
volatile unsigned int YCode_Period=65536;
volatile int Y_Dev=0;
volatile int m_CurrPos[2];
void Y_ENC_SetCounter(int Total_Couner)
{
    if(Total_Couner>=0)
    {
        Y_Encoder_Timer_Overflow=Total_Couner/YCode_Period;
        Y_Encoder_Old_Counter=Total_Couner-(Y_Encoder_Timer_Overflow*YCode_Period);
    }
    else
    {
        Y_Encoder_Timer_Overflow=(Total_Couner+1)/YCode_Period;
        Y_Encoder_Timer_Overflow=Y_Encoder_Timer_Overflow-1;
        Y_Encoder_Old_Counter=Total_Couner-Y_Encoder_Timer_Overflow*YCode_Period;
    }
    Y_Dev=0;
    Y_ENCODER_TIMER->CNT = Y_Encoder_Old_Counter;
}

void Y_ENC_SetPeriod(unsigned int period)
{
    Y_ENCODER_TIMER->ARR = period-1 ; 
    YCode_Period=period;
}
/*******************************************************************************
* Function Name  : ENC_Init
* Description    : General Purpose Timer x set-up for encoder speed/position
*                  sensors
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void Y_ENC_Init()
{    
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;

    /* Encoder unit connected to TIM3, 4X mode */
    GPIO_InitTypeDef GPIO_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure; 
    
    YCode_Period=65536;

    GPIO_StructInit(&GPIO_InitStructure);
    /* Configure PA.06,07 as encoder input */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    //GPIO_PinRemapConfig(GPIO_Remap_TIM4, ENABLE);

    /* Enable the TIM3 Update Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    /* Timer configuration in Encoder mode */
    TIM_DeInit(Y_ENCODER_TIMER);
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

    TIM_TimeBaseStructure.TIM_Prescaler = 0x0;  // No prescaling
    //TIM_TimeBaseStructure.TIM_Period = (4*ENCODER_PPR)-1;
    TIM_TimeBaseStructure.TIM_Period = YCode_Period-1;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(Y_ENCODER_TIMER, &TIM_TimeBaseStructure);

    TIM_EncoderInterfaceConfig(Y_ENCODER_TIMER, TIM_EncoderMode_TI12,
                             TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = Y_ICx_FILTER;
    TIM_ICInit(Y_ENCODER_TIMER, &TIM_ICInitStructure);

    // Clear all pending interrupts
    TIM_ClearFlag(Y_ENCODER_TIMER, TIM_FLAG_Update);
    TIM_ITConfig(Y_ENCODER_TIMER, TIM_IT_Update, ENABLE);
    //Reset counter
    Y_ENCODER_TIMER->CNT = Y_Encoder_Old_Counter;

    TIM_Cmd(Y_ENCODER_TIMER, ENABLE);
    Y_ENC_SetCounter(0);
}



/*******************************************************************************
* Function Name  : ENC_Get_Electrical_Angle
* Description    : Returns the absolute electrical Rotor angle
* Input          : None
* Output         : None
* Return         : Rotor electrical angle: 0 -> 0 degrees,
*                                          S16_MAX-> 180 degrees,
*                                          S16_MIN-> -180 degrees
*******************************************************************************/
int Y_ENC_Get_Counter(void)
{
    //return ((Y_Encoder_Timer_Overflow<<16)+TIM_GetCounter(Y_ENCODER_TIMER));
    return ((Y_Encoder_Timer_Overflow*YCode_Period)+Y_ENCODER_TIMER->CNT+Y_Dev);
}

/*******************************************************************************
* Function Name  : TIM2_IRQHandler
* Description    : This function handles TIMx Update interrupt request.
                   Encoder unit connected to TIM2
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/


void TIM4_IRQHandler(void)
{
    /* Clear the interrupt pending flag */     
    if ( (Y_ENCODER_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)
    {
            Y_Encoder_Timer_Overflow--;
            if(Motor_Info.Dst_Direct==2)
            {                
                Y_Cur_Run_IntCnt++;
                if(Motor_Info.Print_Flag)
                {
                    if(Y_Cur_Run_IntCnt==1)
                    {
                        Enable_FPGA_Print;
                        m_CurrPos[0]=Y_ENC_Get_Counter();
                    }
                    else if(Y_Cur_Run_IntCnt==Y_Run_IntCnt)
                    {
                        Disable_FPGA_Print;  
                        Y_Positive(Y_STOP_PWN);
                        Motor_Info.Dst_Direct=4;
                        m_CurrPos[1]=Y_ENC_Get_Counter();
                    }
                }
                else
                {
                    if(Y_Cur_Run_IntCnt==Y_Run_IntCnt)
                    {                                              
                        Y_Positive(Y_STOP_PWN);
                        Motor_Info.Dst_Direct=4;
                        m_CurrPos[1]=Y_ENC_Get_Counter();
                    }
                }
            }
            else if(Motor_Info.Dst_Direct==1)
            {
                if(Y_Cur_Run_IntCnt>0)
                    Y_Cur_Run_IntCnt--;
            }            
    }
    else
    {       
        Y_Encoder_Timer_Overflow++;  
        if(Motor_Info.Dst_Direct==1)
        {                
            Y_Cur_Run_IntCnt++;
            if(Motor_Info.Print_Flag)
            {
                if(Y_Cur_Run_IntCnt==1)
                {
                    Enable_FPGA_Print;
                    m_CurrPos[0]=Y_ENC_Get_Counter();
                }
                else if(Y_Cur_Run_IntCnt==Y_Run_IntCnt)
                {
                    Disable_FPGA_Print;    
                    Y_Negative(Y_STOP_PWN);                        
                    Motor_Info.Dst_Direct=3;
                    m_CurrPos[1]=Y_ENC_Get_Counter();
                }
            }
            else
            {
                if(Y_Cur_Run_IntCnt==Y_Run_IntCnt)
                {                    
                    Y_Negative(Y_STOP_PWN);                        
                    Motor_Info.Dst_Direct=3;
                    m_CurrPos[1]=Y_ENC_Get_Counter();
                } 
            }
        }
        else if(Motor_Info.Dst_Direct==2)
        {
            if(Y_Cur_Run_IntCnt>0)
                Y_Cur_Run_IntCnt--;
        }                      
    }
    Y_ENCODER_TIMER->SR = 0xfffe;
    //DEBUG_PRINT("%d %d\r\n",Y_ENCODER_TIMER->CNT,Y_Encoder_Timer_Overflow);    
}